/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package ai;

import Jama.Matrix;
import comm.Messenger;
import control.Angvel;
import control.Kalman;
import control.PDController;
import control.Speed;
import control.TankAction;
import control.Utility;
import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStreamReader;
import java.util.ArrayList;
import java.util.Date;
import java.util.List;
import java.util.Random;
import plot.GNUPlot;
import world.Enemy;
import world.Friend;
import world.Point;
import world.Shot;
import world.World;

/**
 *
 * @author dheath
 */
public class ClayPigeon implements Intelligence
{

    private int botId;
    private World world;
    private Messenger msgr;
    private State state;
    private Date lastTime;

    public ClayPigeon(int botId)
    {
        this.botId = botId;
        lastTime = new Date();
        try
        {
            InputStreamReader isr = new InputStreamReader(System.in);
            BufferedReader stdin = new BufferedReader(isr);

            while (true)
            {
                System.out.println("Which clay pigeon?");
                System.out.println("6 - Kalman shooter");
                System.out.println("1 - Sitting duck");
                System.out.println("2 - constant velocity");
                System.out.println("3 - constant acceleration");
                System.out.println("4 - guassien");
                System.out.println("5 - WILD");

                String srch = stdin.readLine();
                int searchv = 0;
                try
                {
                    searchv = Integer.parseInt(srch);
                } catch (NumberFormatException ex)
                {
                    System.out.println("choice must be a number from 1 to 6");
                    continue;
                }
                if (searchv > 0 && searchv < 7)
                {
                    state = setClayPigeonType(searchv);
                    break;
                }
            }




        //stdin.close();
        } catch (IOException e)
        {
            e.printStackTrace();
        }
    }

    public void NextMove()
    {
        state.nextMove(this);
    }

    public int getBotId()
    {
        return botId;
    }

    public World getWorld()
    {
        return world;
    }

    public Messenger getMessenger()
    {
        return msgr;
    }

    public State getState()
    {
        return state;
    }

    public void setBotId(int botId)
    {
        this.botId = botId;
    }

    public void setWorld(World world)
    {
        this.world = world;
    }

    public void setMessenger(Messenger msgr)
    {
        this.msgr = msgr;
    }

    public void setState(State state)
    {
        this.state = state;
    }

    public Date getLastTime()
    {
        return lastTime;
    }

    public void setLastTime(Date lastTime)
    {
        this.lastTime = lastTime;
    }

    private State setClayPigeonType(int type)
    {
        switch (type)
        {
            case 6:
                return new KalmanShooter();
            case 1:
                return new SittingDuck();
            case 2:
                return new ConstantVel();
            case 3:
                return new ConstantAccel();
            case 4:
                return new Gaussian();
            case 5:
                return new Wild();
            default:
                return new SittingDuck();
        }
    }
}

class State
{

    public void nextMove(ClayPigeon bot)
    {

    }
}

class KalmanShooter extends State
{

    protected final double shotRange = 346.0;
    protected final double bulletSpeed = 100.0;
    protected final double noise = 25;
    Kalman enemyLocation;
    Kalman enemyVel; //test
    
    Kalman myLocation;
    List<Point> bufferE;
    List<Point> bufferMe;
    List<Point> bufferV;
    
    private int lastEnemy;
    private GNUPlot plot;
    private int countP = 0;

    public KalmanShooter()
    {
        enemyLocation = new Kalman(noise);
        enemyVel = new Kalman(noise);
        myLocation = new Kalman(noise);

        bufferE = new ArrayList();
        bufferMe = new ArrayList();
        bufferV = new ArrayList();
        
        lastEnemy = 0;
    }

    public void nextMove(ClayPigeon bot)
    {
        int maxPlot = 30;

        if (plot == null)
        {
            plot = new GNUPlot(bot.getWorld(), false, true, "KalmanShooter_" + lastEnemy + ".gpi", .001);
        }

        Friend me = bot.getWorld().getFriends().get(bot.getBotId());

        Enemy e = null;//bot.getWorld().getEnemies().get(0);

        for (int i = 0; i < bot.getWorld().getEnemies().size(); i++)
        {
            Enemy en = bot.getWorld().getEnemies().get(i);
            if (!en.getStatus().equalsIgnoreCase("dead"))// && Utility.distance(me.getLocation(), en.getLocation()) <= shotRange)
            {
                countP++;
                e = en;
                if (i != lastEnemy)
                {
                    enemyLocation.resetFilter(noise);
                    enemyVel.resetFilter(noise);
                }
                lastEnemy = i;
                break;
            }
        }
        if (e == null)
        {
            e = bot.getWorld().getEnemies().get(0);
            enemyLocation.resetFilter(noise);
            enemyVel.resetFilter(noise);
            lastEnemy = 0;
            plot.closeConnection();
        }
        if(countP % 30 == 0)
        {
            myLocation.resetFilter(noise);
            enemyLocation.resetFilter(noise);
            enemyVel.resetFilter(noise);
        }
        if (countP >= maxPlot)
        {
            plot.closeConnection();
        }
        Date now = new Date();
        double dT = (now.getTime() - bot.getLastTime().getTime()) / 1000.0;
        //System.out.println("Time from last time: "+dT);
        bot.setLastTime(now);


        bufferE.add(e.getLocation());
        bufferMe.add(me.getLocation());

        if (bufferE.size() > 2)
        {
            Point[] enInfo = getEstimatedPosition(bufferE, bot, dT, enemyLocation);
            if (countP < maxPlot)
            {
                enemyLocation.plotMultivariateNormal(plot, dT);
            }
            Point enPos = enInfo[0];

            Point pme = getEstimatedPosition(bufferMe, bot, dT, myLocation)[0];
            
            
            
            Point ev = enInfo[1];//new Point(enInfo[1].getX()*10, enInfo[1].getY()*10);
            bufferV.add(ev);
            if(bufferV.size() > 2)
            {
                ev = getEstimatedPosition(bufferV, bot, dT, enemyVel)[0];
                bufferV.remove(0);
            }
            double shotSec = getPredictingTime(enPos, pme, ev, dT);
            //double shotSec = getPredictingTime(e.getLocation(),me.getLocation(), enInfo[1],dT);

            System.out.println("predictedPos: " + enPos + " PredictedV: " + ev);
            //System.out.println(ev+" ----- "+shotSec);

            Point en = getFuturePosition(shotSec, enemyLocation);

            if (countP < maxPlot && countP >= maxPlot - 2)
            {
                plot.printX(enPos, 2);
                plot.printX(en, 1);
            }
            List<TankAction> moves = PDController.move(me.getAngle(), pme, en, (long) (dT * 1000.0));

            for (TankAction a : moves)
            {
                if (!(a instanceof Speed))
                {
                    a.performAction(bot.getMessenger(), bot.getBotId());
                }
            }

            if (countP < maxPlot)
            {
                plot.endTimeFrame();
            }

            bufferE.remove(0);
            bufferMe.remove(0);
        }



    }

    private double getPredictingTime(Point enemy, Point me, Point enemyV, double dT)
    {
        //(s^2-(v.v))*t^2 - (2*((A-B).v))*t - (A-B).(A-B) = 0
        Point temp = Utility.minus(enemy, me);

        double a = (bulletSpeed) * (bulletSpeed) - Utility.dot(enemyV, enemyV);
        double b = -2 * (Utility.dot(temp, enemyV));
        double c = -1 * Utility.dot(temp, temp);

        //System.out.println("a,b,c: "+a+" "+b+" "+c);
        return Utility.root(a, b, c);
    }

    private Point getFuturePosition(double dT, Kalman filter)
    {
        Matrix Ut = filter.getFuturePrediction(dT);

        return new Point(Ut.get(0, 0), Ut.get(3, 0));
    }

    private Point[] getEstimatedPosition(List<Point> buffer, ClayPigeon bot, double dT, Kalman filter)
    {

        double vOldx = (buffer.get(1).getX() - buffer.get(0).getX()) / dT;
        double vOldy = (buffer.get(1).getY() - buffer.get(0).getY()) / dT;

        double vNewx = (buffer.get(2).getX() - buffer.get(1).getX()) / dT;
        double vNewy = (buffer.get(2).getY() - buffer.get(1).getY()) / dT;

        double accx = (vNewx - vOldx) / dT;
        double accy = (vNewy - vOldy) / dT;

        double[][] zz = {{buffer.get(2).getX()}, {vNewx}, {accx}, {buffer.get(2).getY()}, {vNewy}, {accy}};
        Matrix Zt = new Matrix(zz);

        Matrix Ut = filter.updateFilter(Zt, dT);


        Point[] res = new Point[2];
        res[0] = new Point(Ut.get(0, 0), Ut.get(3, 0));
        res[1] = new Point(Ut.get(1, 0), Ut.get(4, 0));

        return res;

    }
}

class SittingDuck extends State
{

    private Point lastPoint = new Point(0, 0);

    public void nextMove(ClayPigeon bot)
    {
        Date now = new Date();
        double dT = (now.getTime() - bot.getLastTime().getTime()) / 1000.0;
        bot.setLastTime(now);

        Shot s = null;
        for (Shot ss : bot.getWorld().getShots())
        {
            s = ss;
            break;
        }
        if (s != null)
        {
            double dist = Utility.distance(s.getLocation(), lastPoint);

            lastPoint = s.getLocation();
            System.out.println("distance: " + dist + " dT: " + dT + " speed: " + (dist / dT));
        }

    }
}

class ConstantVel extends State
{

    private double baseAngle;
    private boolean turning;

    public ConstantVel()
    {
        baseAngle = 0.0;
        turning = true;
    }

    public void nextMove(ClayPigeon bot)
    {
        Friend me = bot.getWorld().getFriends().get(bot.getBotId());
        System.out.println("actualPos: " + me.getLocation() + " actualV: (" + me.getVx() + "," + me.getVy() + ")");
        if (baseAngle == 0.0)
        {
            (new Speed(.4)).performAction(bot.getMessenger(), bot.getBotId());
        }

        if (turning)
        {
            if (Math.abs(baseAngle - me.getAngle()) >= .9)
            {
                turning = false;
                (new Angvel(0.0)).performAction(bot.getMessenger(), bot.getBotId());
                (new Speed(.8)).performAction(bot.getMessenger(), bot.getBotId());
            } else
            {
                (new Angvel(0.9)).performAction(bot.getMessenger(), bot.getBotId());
            }
        } else
        {

            //System.out.println()
           	double wall = 800.0 / 2.0 - 6.0;
        	double currAng = Utility.getRadians02PI(me.getAngle());
        	if(  // if tank is pointed in direction of wall he is against
        			(          0 < currAng &&       currAng <= Math.PI &&  me.getLocation().getY() > wall)      ||
        			(Math.PI / 2 < currAng && currAng <= 3 * Math.PI/2 &&  me.getLocation().getX() < -1 * wall) ||
        			(    Math.PI < currAng && currAng <=   2 * Math.PI &&  me.getLocation().getY() < -1 * wall) ||
        			( (currAng >= 3*Math.PI/2 || currAng <= Math.PI/2) &&  me.getLocation().getX() > wall)
        	)
       //    if (Math.abs(me.getVx()) <= .07 || Math.abs(me.getVy()) <= .07 || me.getLocation().getX() < -1 * wall || me.getLocation().getX() > wall || me.getLocation().getY() < -1 * wall || me.getLocation().getY() > wall)
            {
                (new Angvel(1.0)).performAction(bot.getMessenger(), bot.getBotId());
                (new Speed(-.1)).performAction(bot.getMessenger(), bot.getBotId());
                turning = true;
                baseAngle = me.getAngle();
            } else
            {
                (new Speed(.8)).performAction(bot.getMessenger(), bot.getBotId());
            }
        }
    }
}

class ConstantAccel extends State
{

    private double baseAngle;
    private boolean turning;

    public ConstantAccel()
    {
        baseAngle = 0.0;
        turning = true;
    }

    public void nextMove(ClayPigeon bot)
    {
        Friend me = bot.getWorld().getFriends().get(bot.getBotId());

        if (baseAngle == 0.0)
        {
            (new Speed(.4)).performAction(bot.getMessenger(), bot.getBotId());
            (new Angvel(.3)).performAction(bot.getMessenger(), bot.getBotId());
        }

        if (turning)
        {
            if (Math.abs(baseAngle - me.getAngle()) >= .9)
            {
                turning = false;
                (new Angvel(0.3)).performAction(bot.getMessenger(), bot.getBotId());
                (new Speed(1)).performAction(bot.getMessenger(), bot.getBotId());
            } else
            {
                (new Angvel(0.7)).performAction(bot.getMessenger(), bot.getBotId());
            }
        } else
        {

            //System.out.println()
        	double wall = 800.0 / 2.0 - 6.0;
        	double currAng = Utility.getRadians02PI(me.getAngle());
        	if(  // if tank is pointed in direction of wall he is against
        			(          0 < currAng &&       currAng <= Math.PI &&  me.getLocation().getY() > wall)      ||
        			(Math.PI / 2 < currAng && currAng <= 3 * Math.PI/2 &&  me.getLocation().getX() < -1 * wall) ||
        			(    Math.PI < currAng && currAng <=   2 * Math.PI &&  me.getLocation().getY() < -1 * wall) ||
        			( (currAng >= 3*Math.PI/2 || currAng <= Math.PI/2) &&  me.getLocation().getX() > wall)
        	)
          //  if (Math.abs(me.getVx()) <= .07 || Math.abs(me.getVy()) <= .07 || me.getLocation().getX() < -1 * wall || me.getLocation().getX() > wall || me.getLocation().getY() < -1 * wall || me.getLocation().getY() > wall)
            {
                (new Angvel(1.0)).performAction(bot.getMessenger(), bot.getBotId());
                (new Speed(-.1)).performAction(bot.getMessenger(), bot.getBotId());
                turning = true;
                baseAngle = me.getAngle();
            } else
            {
                (new Speed(1)).performAction(bot.getMessenger(), bot.getBotId());
                (new Angvel(.3)).performAction(bot.getMessenger(), bot.getBotId());
            }
        }
    }
}

class Gaussian extends State
{

    private Random gen;
    private final double stdev = 0.3;
    // private Kalman kfilter;
    private Matrix X;
    private boolean init;
    private PDController controller;
    private boolean turning;
    private double targetAngle;
    public Gaussian()
    {
        gen = new Random();
        // kfilter = new Kalman();
        double[][] initX = {{0}, {0}, {0}, {0}, {0}, {0}};
        X = new Matrix(initX);
        init = true;
        controller = new PDController();
        turning = false;
        targetAngle = 0;
    }

    public void nextMove(ClayPigeon bot)
    {
    //	System.out.println("Next move for gaussian");
        Friend me = bot.getWorld().getFriends().get(bot.getBotId());
        if (init)
        {
            X.set(0, 0, me.getLocation().getX());
            X.set(3, 0, me.getLocation().getY());
            init = false;
        }
        else if (turning)
        {
        	double currAng = Utility.getRadians02PI(me.getAngle());

    		//System.out.println("Current Angle:" + currAng);
        	if(targetAngle - .5 < currAng && currAng < targetAngle) {
        		turning = false;
        		(new Angvel(0)).performAction(bot.getMessenger(), bot.getBotId());
        	}
        	else
        	{
        		(new Speed(-0.4)).performAction(bot.getMessenger(), bot.getBotId());
        		(new Angvel(.5)).performAction(bot.getMessenger(), bot.getBotId());
        	}
        }
        else
        {
        	double wall = 800.0 / 2.0 - 6.0;
        	double currAng = Utility.getRadians02PI(me.getAngle());
        	if(  // if tank is pointed in direction of wall he is against
        			(          0 < currAng &&       currAng <= Math.PI &&  me.getLocation().getY() > wall)      ||
        			(Math.PI / 2 < currAng && currAng <= 3 * Math.PI/2 &&  me.getLocation().getX() < -1 * wall) ||
        			(    Math.PI < currAng && currAng <=   2 * Math.PI &&  me.getLocation().getY() < -1 * wall) ||
        			( (currAng >= 3*Math.PI/2 || currAng <= Math.PI/2) &&  me.getLocation().getX() > wall)
        	)
        	{
        		(new Speed(-0.2)).performAction(bot.getMessenger(), bot.getBotId());
        		(new Angvel(0)).performAction(bot.getMessenger(), bot.getBotId());
        		turning = true;
        		targetAngle = me.getAngle() + Utility.degreeToRadians(90);
        		if (targetAngle >= 2 * Math.PI)
        		{
        			targetAngle -= 2 * Math.PI;
        		}
        //		System.out.println("New Target Angle:" + targetAngle);
        		
        	}
            Point currVel = new Point(X.get(1, 0), X.get(4, 0));
            Date now = new Date();
            double dT = .001 * (now.getTime() - bot.getLastTime().getTime()); // in seconds
            bot.setLastTime(now);
            double gaussian = gen.nextGaussian();
           // System.out.println("Gaussian: " + gaussian);
            X.set(2, 0, gen.nextGaussian() * stdev);
            X.set(5, 0, gen.nextGaussian() * stdev);
            if(Math.abs(X.get(1,0)) >= 1 || Math.abs(X.get(4, 0)) >= 1) // normalize velocities if necessary
            {
            	double xv = X.get(1,0);
            	double yv = X.get(4, 0);
            	double divisor = xv * xv + yv * yv;
            	X.set(1,0,xv/divisor);
            	X.set(4,0,yv/divisor);
            }
            double[][] initF = {{1.0, dT, dT * dT / 2, 0, 0, 0},
                {0, 1, dT, 0, 0, 0},
                {0, .1, 1, 0, 0, 0},
                {0, 0, 0, 1.0, dT, dT * dT / 2},
                {0, 0, 0, 0, 1, dT},
                {0, 0, 0, 0, .1, 1}};
            double[][] initN = {{0},
                {0},
                {0},
                {0},
                {0},
                {0}};
            Matrix F = new Matrix(initF);
            Matrix N = new Matrix(initN);
            X = F.times(X);
            X = X.plus(N);
            // bot should do this:
            // have a certain velocity
            // have a direction of the velocity.

            Point newVel = new Point(X.get(1, 0), X.get(4, 0));
          //  System.out.println("New Velocity: "+newVel);
        //	System.out.println("getting moves");
            List<TankAction> actions = controller.move(currVel, newVel, (dT * 1000));

        //	System.out.println("done getting moves");
            for (TankAction action : actions)
            {
                action.performAction(bot.getMessenger(), me.getBotId());
            }
        }

    }
}

class Wild extends State
{

    private Date lastTime;
    private double baseAngle;
    private boolean turning;

    public Wild()
    {
        baseAngle = 0.0;
        turning = true;
        lastTime = new Date();
    }

    public void nextMove(ClayPigeon bot)
    {
        Date now = new Date();
        System.out.println("time delay: " + (now.getTime() - lastTime.getTime()));
        lastTime = now;

        Random rand = new Random();
        double factor = 1.0;
        if (rand.nextBoolean())
        {
            factor = -1.0;
        }
        Friend me = bot.getWorld().getFriends().get(bot.getBotId());

        if (baseAngle == 0.0)
        {
            (new Speed(.4 + rand.nextDouble())).performAction(bot.getMessenger(), bot.getBotId());
            (new Angvel((.2 + rand.nextDouble()) * factor)).performAction(bot.getMessenger(), bot.getBotId());
        }

        if (turning)
        {
            if (Math.abs(baseAngle - me.getAngle()) >= .9)
            {
                turning = false;
                (new Angvel((0.2 + rand.nextDouble()))).performAction(bot.getMessenger(), bot.getBotId());
                (new Speed(.8 + rand.nextDouble())).performAction(bot.getMessenger(), bot.getBotId());
            } else
            {
                (new Angvel(0.7)).performAction(bot.getMessenger(), bot.getBotId());
            }
        } else
        {

            //System.out.println()
           	double wall = 800.0 / 2.0 - 6.0;
        	double currAng = Utility.getRadians02PI(me.getAngle());
        	if(  // if tank is pointed in direction of wall he is against
        			(          0 < currAng &&       currAng <= Math.PI &&  me.getLocation().getY() > wall)      ||
        			(Math.PI / 2 < currAng && currAng <= 3 * Math.PI/2 &&  me.getLocation().getX() < -1 * wall) ||
        			(    Math.PI < currAng && currAng <=   2 * Math.PI &&  me.getLocation().getY() < -1 * wall) ||
        			( (currAng >= 3*Math.PI/2 || currAng <= Math.PI/2) &&  me.getLocation().getX() > wall)
        	)

          //  if (Math.abs(me.getVx()) <= .07 || Math.abs(me.getVy()) <= .07 || me.getLocation().getX() < -1 * wall || me.getLocation().getX() > wall || me.getLocation().getY() < -1 * wall || me.getLocation().getY() > wall)
            {
                (new Angvel(1.0)).performAction(bot.getMessenger(), bot.getBotId());
                (new Speed(-.1)).performAction(bot.getMessenger(), bot.getBotId());
                turning = true;
                baseAngle = me.getAngle();
            } else
            {
                (new Speed(.8 + rand.nextDouble())).performAction(bot.getMessenger(), bot.getBotId());
                (new Angvel((.2 + rand.nextDouble()) * factor)).performAction(bot.getMessenger(), bot.getBotId());
            }
        }
    }
}
